#include <caros/ur_action_interface.h>
#include <caros/common.h>
#include <caros/common_robwork.h>

#include <rw/math.hpp>
#include <rw/trajectory/Path.hpp>

#include <tf/tf.h>

#include <vector>
#include <stdexcept>
#include <tuple>
#include <string>
#include <algorithm>

using caros::URActionInterface;

URActionInterface::URActionInterface(ros::NodeHandle nh)
    : nh_(nh, UR_ACTION_INTERFACE_SUB_NAMESPACE),
      as_force_mode_start_(nh_, "force_mode_start", boost::bind(&URActionInterface::forceModeStartCB, this, _1),
                           ACTION_SERVER_NO_AUTOSTART),
      as_force_mode_stop_(nh_, "force_mode_stop", boost::bind(&URActionInterface::forceModeStopCB, this, _1),
                          ACTION_SERVER_NO_AUTOSTART),
      as_force_mode_update_(nh_, "force_mode_update", boost::bind(&URActionInterface::forceModeUpdateCB, this, _1),
                            ACTION_SERVER_NO_AUTOSTART),
      as_move_lin_(nh_, "move_lin", boost::bind(&URActionInterface::moveLinCB, this, _1), ACTION_SERVER_NO_AUTOSTART),
      as_move_ptp_(nh_, "move_ptp", boost::bind(&URActionInterface::movePtpCB, this, _1), ACTION_SERVER_NO_AUTOSTART),
      as_move_ptp_path_(nh_, "move_ptp_path", boost::bind(&URActionInterface::movePtpPathCB, this, _1),
                        ACTION_SERVER_NO_AUTOSTART),
      as_move_servo_q_(nh_, "move_servo_q", boost::bind(&URActionInterface::moveServoQCB, this, _1),
                       ACTION_SERVER_NO_AUTOSTART),
      as_move_servo_stop_(nh_, "move_servo_stop", boost::bind(&URActionInterface::moveServoStopCB, this, _1),
                          ACTION_SERVER_NO_AUTOSTART),
      as_move_stop_(nh_, "move_stop", boost::bind(&URActionInterface::moveStopCB, this, _1),
                    ACTION_SERVER_NO_AUTOSTART),
      as_move_vel_q_(nh_, "move_vel_q", boost::bind(&URActionInterface::moveVelQCB, this, _1),
                     ACTION_SERVER_NO_AUTOSTART),
      as_move_vel_t_(nh_, "move_vel_t", boost::bind(&URActionInterface::moveVelTCB, this, _1),
                     ACTION_SERVER_NO_AUTOSTART),
      as_move_vel_stop_(nh_, "move_vel_stop", boost::bind(&URActionInterface::moveVelStopCB, this, _1),
                     ACTION_SERVER_NO_AUTOSTART),
      as_set_io_(nh_, "set_io", boost::bind(&URActionInterface::setIOCB, this, _1), ACTION_SERVER_NO_AUTOSTART),
      as_set_payload_(nh_, "set_payload", boost::bind(&URActionInterface::setPayloadCB, this, _1),
                      ACTION_SERVER_NO_AUTOSTART),
      as_zero_ftsensor_(nh_, "zero_ftsensor", boost::bind(&URActionInterface::zeroFTsensorCB, this, _1),
                        ACTION_SERVER_NO_AUTOSTART),
      as_send_custom_script_function_(nh_, "send_custom_script_function",
                                      boost::bind(&URActionInterface::sendCustomScriptFunctionCB, this, _1),
                                      ACTION_SERVER_NO_AUTOSTART),
      as_send_custom_script_file_(nh_, "send_custom_script_file",
                                  boost::bind(&URActionInterface::sendCustomScriptFileCB, this, _1),
                                  ACTION_SERVER_NO_AUTOSTART)

{
  /* Init the robot state publisher */
  ur_state_publisher_ = nh_.advertise<caros_control_msgs::RobotState>("robot_state", UR_STATE_PUBLISHER_QUEUE_SIZE);
  ROS_ERROR_STREAM_COND(!ur_state_publisher_, "The RobotState publisher is empty!");

  /* Start the action servers */
  as_force_mode_start_.start();
  ROS_DEBUG("force_mode_start successfully initialized! Ready for action...");
  as_force_mode_stop_.start();
  ROS_DEBUG("force_mode_stop successfully initialized! Ready for action...");
  as_force_mode_update_.start();
  ROS_DEBUG("force_mode_update successfully initialized! Ready for action...");
  as_move_lin_.start();
  ROS_DEBUG("move_lin successfully initialized! Ready for action...");
  as_move_ptp_.start();
  ROS_DEBUG("move_ptp successfully initialized! Ready for action...");
  as_move_ptp_path_.start();
  ROS_DEBUG("move_ptp_path successfully initialized! Ready for action...");
  as_move_servo_q_.start();
  ROS_DEBUG("move_servo_q successfully initialized! Ready for action...");
  as_move_servo_stop_.start();
  ROS_DEBUG("move_servo_stop successfully initialized! Ready for action...");
  as_move_stop_.start();
  ROS_DEBUG("move_stop successfully initialized! Ready for action...");
  as_move_vel_q_.start();
  ROS_DEBUG("move_vel_q successfully initialized! Ready for action...");
  as_move_vel_t_.start();
  ROS_DEBUG("move_vel_t successfully initialized! Ready for action...");
  as_move_vel_stop_.start();
  ROS_DEBUG("move_vel_stop successfully initialized! Ready for action...");
  as_set_io_.start();
  ROS_DEBUG("set_io successfully initialized! Ready for action...");
  as_set_payload_.start();
  ROS_DEBUG("set_payload successfully initialized! Ready for action...");
  as_zero_ftsensor_.start();
  ROS_DEBUG("zero_ftsensor successfully initialized! Ready for action...");
  as_send_custom_script_function_.start();
  ROS_DEBUG("send_custom_script_function successfully initialized! Ready for action...");
  as_send_custom_script_file_.start();
  ROS_DEBUG("send_custom_script_file successfully initialized! Ready for action...");
}

URActionInterface::~URActionInterface()
{
  /* Upon destruction shutdown action servers */
  as_force_mode_start_.shutdown();
  as_force_mode_stop_.shutdown();
  as_force_mode_update_.shutdown();
  as_move_lin_.shutdown();
  as_move_ptp_.shutdown();
  as_move_ptp_path_.shutdown();
  as_move_servo_q_.shutdown();
  as_move_servo_stop_.shutdown();
  as_move_stop_.shutdown();
  as_move_vel_q_.shutdown();
  as_move_vel_t_.shutdown();
  as_move_vel_stop_.shutdown();
  as_set_io_.shutdown();
  as_set_payload_.shutdown();
  as_zero_ftsensor_.shutdown();
  as_send_custom_script_function_.shutdown();
  as_send_custom_script_file_.shutdown();
}

/************************************************************************
 * ROS action callback functions
 ************************************************************************/
void URActionInterface::publishState(const caros_control_msgs::RobotState &state)
{
  ur_state_publisher_.publish(state);
}

void URActionInterface::forceModeStartCB(const caros_universalrobot::ForceModeStartGoalConstPtr &goal)
{
  bool success = true;

  rw::math::Transform3D<> ref_t_offset = caros::toRw(goal->base2forceFrame);
  rw::math::Wrench6D<> wrench_target;
  wrench_target(0) = goal->wrench.force.x;
  wrench_target(1) = goal->wrench.force.y;
  wrench_target(2) = goal->wrench.force.z;

  wrench_target(3) = goal->wrench.torque.x;
  wrench_target(4) = goal->wrench.torque.y;
  wrench_target(5) = goal->wrench.torque.z;

  std::size_t index;
  rw::math::Q selection(goal->selection.size());
  index = 0;
  for (const auto item : goal->selection)
  {
    selection(index++) = static_cast<double>(item);
  }

  rw::math::Q limits(goal->limits.size());
  index = 0;
  for (const auto item : goal->limits)
  {
    limits(index++) = static_cast<double>(item);
  }

  // publish info to the console for the user
  ROS_INFO("force_mode_start: Executing ...");

  // start executing the action
  success = urForceModeStart(ref_t_offset, selection, wrench_target, 2, limits);

  if (success)
  {
    force_mode_start_result_.result.error_code = 0;
    force_mode_start_result_.result.error_msg = "";
    force_mode_start_result_.result.succeeded = static_cast<unsigned char>(true);
    ROS_INFO("force_mode_start: Succeeded!");
    // set the action state to succeeded
    as_force_mode_start_.setSucceeded(force_mode_start_result_);
  }
  else
  {
    force_mode_start_result_.result.error_code = 1;
    force_mode_start_result_.result.error_msg = "urForceModeStart failed to execute";
    force_mode_start_result_.result.succeeded = static_cast<unsigned char>(false);
    ROS_WARN("force_mode_start: Failed");
    as_force_mode_start_.setAborted(force_mode_start_result_, "urForceModeStart failed to execute");
  }
}

void URActionInterface::forceModeStopCB(const caros_universalrobot::ForceModeStopGoalConstPtr &goal)
{
  bool success = true;

  // publish info to the console for the user
  ROS_INFO("force_mode_stop: Executing ...");

  // start executing the action
  success = urForceModeStop();

  if (success)
  {
    force_mode_stop_result_.result.error_code = 0;
    force_mode_stop_result_.result.error_msg = "";
    force_mode_stop_result_.result.succeeded = static_cast<unsigned char>(true);
    ROS_INFO("force_mode_stop: Succeeded!");
    // set the action state to succeeded
    as_force_mode_stop_.setSucceeded(force_mode_stop_result_);
  }
  else
  {
    force_mode_stop_result_.result.error_code = 1;
    force_mode_stop_result_.result.error_msg = "urForceModeStop failed to execute";
    force_mode_stop_result_.result.succeeded = static_cast<unsigned char>(false);
    ROS_WARN("force_mode_stop: Failed");
    as_force_mode_stop_.setAborted(force_mode_stop_result_, "urForceModeStop failed to execute");
  }
}

void URActionInterface::forceModeUpdateCB(const caros_universalrobot::ForceModeUpdateGoalConstPtr &goal)
{
  bool success = true;

  rw::math::Wrench6D<> wrench_target = caros::toRw(goal->wrench);

  // publish info to the console for the user
  ROS_INFO_STREAM("force_mode_update: Executing, with wrench: " << wrench_target);

  // start executing the action
  success = urForceModeUpdate(wrench_target);

  if (success)
  {
    force_mode_update_result_.result.error_code = 0;
    force_mode_update_result_.result.error_msg = "";
    force_mode_update_result_.result.succeeded = static_cast<unsigned char>(true);
    ROS_INFO("force_mode_update: Succeeded!");
    // set the action state to succeeded
    as_force_mode_update_.setSucceeded(force_mode_update_result_);
  }
  else
  {
    force_mode_update_result_.result.error_code = 1;
    force_mode_update_result_.result.error_msg = "urForceModeUpdate failed to execute";
    force_mode_update_result_.result.succeeded = static_cast<unsigned char>(false);
    ROS_WARN("force_mode_update: Failed");
    as_force_mode_update_.setAborted(force_mode_update_result_, "urForceModeUpdate failed to execute");
  }
}

void URActionInterface::moveLinCB(const caros_universalrobot::MoveLinGoalConstPtr &goal)
{
  bool success = true;

  // parse speed and acceleration
  const double tool_acceleration = goal->tool_acceleration;
  const double speed = goal->speed;

  // parse the target pose
  rw::math::Vector3D<> pos(goal->tcp_pose.translation.x, goal->tcp_pose.translation.y, goal->tcp_pose.translation.z);

  rw::math::Quaternion<> quaternion(goal->tcp_pose.rotation.x, goal->tcp_pose.rotation.y, goal->tcp_pose.rotation.z,
                                    goal->tcp_pose.rotation.w);

  rw::math::Transform3D<> transform(pos, quaternion.toRotation3D());

  // publish info to the console for the user
  ROS_INFO_STREAM("move_lin: Executing, calling moveLin with goal transform: " << transform);

  // start executing the action
  success = urMoveLin(transform, speed, tool_acceleration);

  if (success)
  {
    move_lin_result_.result.error_code = 0;
    move_lin_result_.result.error_msg = "";
    move_lin_result_.result.succeeded = static_cast<unsigned char>(true);
    ROS_INFO("move_lin: Succeeded!");
    // set the action state to succeeded
    as_move_lin_.setSucceeded(move_lin_result_);
  }
  else
  {
    move_lin_result_.result.error_code = 1;
    move_lin_result_.result.error_msg = "moveLin failed to execute";
    move_lin_result_.result.succeeded = static_cast<unsigned char>(false);
    ROS_WARN("move_lin: Failed");
    as_move_lin_.setAborted(move_lin_result_, "moveLin failed to execute");
  }
}

void URActionInterface::movePtpCB(const caros_universalrobot::MovePtpGoalConstPtr &goal)
{
  bool success = true;

  // parse speed and accceleration
  const double speed = goal->speed;
  const double acceleration = goal->acceleration;

  // parse the joint configurations
  rw::math::Q q(goal->joint_pose.data.size());
  for (unsigned int i = 0; i < goal->joint_pose.data.size(); i++)
  {
    q[i] = goal->joint_pose.data[i];
  }

  // publish info to the console for the user
  ROS_INFO_STREAM("move_ptp: Executing, calling movePtp with goal q: " << q);

  // start executing the action
  success = urMovePtp(q, speed, acceleration);

  /*for (unsigned int index = 0; index < goal->joints[0].position.size(); index++)
    move_ptp_feedback_.joint_pose.push_back(static_cast<float>(0.0));

  // publish the feedback
  as_move_ptp_.publishFeedback(move_ptp_feedback_);*/

  if (success)
  {
    move_ptp_result_.result.error_code = 0;
    move_ptp_result_.result.error_msg = "";
    move_ptp_result_.result.succeeded = static_cast<unsigned char>(true);
    ROS_INFO("move_ptp: Succeeded!");
    // set the action state to succeeded
    as_move_ptp_.setSucceeded(move_ptp_result_);
  }
  else
  {
    move_ptp_result_.result.error_code = 1;
    move_ptp_result_.result.error_msg = "movePtp failed to execute";
    move_ptp_result_.result.succeeded = static_cast<unsigned char>(false);
    ROS_WARN("move_ptp: Failed");
    as_move_ptp_.setAborted(move_ptp_result_, "movePtp failed to execute");
  }
}

void URActionInterface::movePtpPathCB(const caros_universalrobot::MovePtpPathGoalConstPtr &goal)
{
  bool success = true;

  // parse the joint poses in path with speeds, accelerations and blends.
  rw::trajectory::QPath q_path;

  try
  {
    for (unsigned int index = 0; index < goal->joint_poses.size(); index++)
    {
      rw::math::Q q(9);
      double speed = goal->speeds[index];
      double acceleration = goal->accelerations[index];
      double blend = goal->blends[index];
      for (unsigned int i = 0; i < goal->joint_poses[0].data.size(); i++)
      {
        q[i] = goal->joint_poses[index].data[i];
      }
      q[6] = speed;
      q[7] = acceleration;
      q[8] = blend;
      q_path.push_back(q);
    }
  }
  catch (const std::out_of_range &oor)
  {
    ROS_ERROR_STREAM("Container Out of Range error: " << oor.what());
  }

  // publish info to the console for the user
  ROS_INFO("move_ptp_path: Executing, calling movePtpPath with q_path");

  // start executing the action
  if (!q_path.empty())
  {
    success = urMovePtpPath(q_path);
  }

  /*for (unsigned int index = 0; index < goal->joints[0].position.size(); index++)
    move_ptp_feedback_.joint_pose.push_back(static_cast<float>(0.0));

  // publish the feedback
  as_move_ptp_.publishFeedback(move_ptp_feedback_);*/

  if (success)
  {
    move_ptp_path_result_.result.error_code = 0;
    move_ptp_path_result_.result.error_msg = "";
    move_ptp_path_result_.result.succeeded = static_cast<unsigned char>(true);
    ROS_INFO("move_ptp_path: Succeeded!");
    // set the action state to succeeded
    as_move_ptp_path_.setSucceeded(move_ptp_path_result_);
  }
  else
  {
    move_ptp_path_result_.result.error_code = 1;
    move_ptp_path_result_.result.error_msg = "movePtpPath failed to execute";
    move_ptp_path_result_.result.succeeded = static_cast<unsigned char>(false);
    ROS_WARN("move_ptp_path: Failed");
    as_move_ptp_path_.setAborted(move_ptp_path_result_, "movePtpPath failed to execute");
  }
}

void URActionInterface::moveServoQCB(const caros_universalrobot::MoveServoQGoalConstPtr &goal)
{
  bool success = true;

  // parse parameters
  double time = goal->time;
  double lookahead_time = goal->lookahead_time;
  double gain = goal->gain;

  rw::math::Q q(goal->joint_pose.data.size());

  for (std::size_t i = 0; i < q.size(); i++)
  {
    q[i] = goal->joint_pose.data[i];
  }

  // publish info to the console for the user
  ROS_INFO_STREAM("move_servo_q: Executing, calling moveServoPtp with goal: " << goal);

  // start executing the action
  success = urServoQ(q, time, lookahead_time, gain);

  if (success)
  {
    move_servo_q_result_.result.error_code = 0;
    move_servo_q_result_.result.error_msg = "";
    move_servo_q_result_.result.succeeded = static_cast<unsigned char>(true);
    ROS_INFO("move_servo_q: Succeeded!");
    // set the action state to succeeded
    as_move_servo_q_.setSucceeded(move_servo_q_result_);
  }
  else
  {
    move_servo_q_result_.result.error_code = 1;
    move_servo_q_result_.result.error_msg = "moveServoQ failed to execute";
    move_servo_q_result_.result.succeeded = static_cast<unsigned char>(false);
    ROS_WARN("move_servo_q: Failed");
    as_move_servo_q_.setAborted(move_servo_q_result_, "moveServoQ failed to execute");
  }
}

void URActionInterface::moveServoStopCB(const caros_universalrobot::MoveServoStopGoalConstPtr &goal)
{
  bool success = true;

  // publish info to the console for the user
  ROS_INFO_STREAM("move_servo_stop: Executing, calling moveServoStop");

  // start executing the action
  success = urServoStop();

  if (success)
  {
    move_servo_stop_result_.result.error_code = 0;
    move_servo_stop_result_.result.error_msg = "";
    move_servo_stop_result_.result.succeeded = static_cast<unsigned char>(true);
    ROS_INFO("move_servo_stop: Succeeded!");
    // set the action state to succeeded
    as_move_servo_stop_.setSucceeded(move_servo_stop_result_);
  }
  else
  {
    move_servo_stop_result_.result.error_code = 1;
    move_servo_stop_result_.result.error_msg = "moveServoStop failed to execute";
    move_servo_stop_result_.result.succeeded = static_cast<unsigned char>(false);
    ROS_WARN("move_servo_stop: Failed");
    as_move_servo_stop_.setAborted(move_servo_stop_result_, "moveServoStop failed to execute");
  }
}

void URActionInterface::moveStopCB(const caros_universalrobot::MoveStopGoalConstPtr &goal)
{
  bool success = true;

  // publish info to the console for the user
  ROS_INFO_STREAM("move_stop: Executing, calling urMoveStop");

  // start executing the action
  success = urMoveStop();

  if (success)
  {
    move_stop_result_.result.error_code = 0;
    move_stop_result_.result.error_msg = "";
    move_stop_result_.result.succeeded = static_cast<unsigned char>(true);
    ROS_INFO("move_stop: Succeeded!");
    // set the action state to succeeded
    as_move_stop_.setSucceeded(move_stop_result_);
  }
  else
  {
    move_stop_result_.result.error_code = 1;
    move_stop_result_.result.error_msg = "urMoveStop failed to execute";
    move_stop_result_.result.succeeded = static_cast<unsigned char>(false);
    ROS_WARN("move_stop: Failed");
    as_move_stop_.setAborted(move_stop_result_, "urMoveStop failed to execute");
  }
}

void URActionInterface::moveVelQCB(const caros_universalrobot::MoveVelQGoalConstPtr &goal)
{
  bool success = true;
  double acceleration = goal->acceleration;
  double time = goal->time;

  rw::math::Q qd(goal->joint_velocity.data.size());
  for (std::size_t i = 0; i < qd.size(); i++)
  {
    qd[i] = goal->joint_velocity.data[i];
  }

  // publish info to the console for the user
  ROS_INFO_STREAM("move_vel_q: Executing, calling moveVelQ with goal velocities: " << qd);

  // start executing the action
  success = urMoveVelQ(qd, acceleration, time);

  if (success)
  {
    move_vel_q_result_.result.error_code = 0;
    move_vel_q_result_.result.error_msg = "";
    move_vel_q_result_.result.succeeded = static_cast<unsigned char>(true);
    ROS_INFO("move_vel_q: Succeeded!");
    // set the action state to succeeded
    as_move_vel_q_.setSucceeded(move_vel_q_result_);
  }
  else
  {
    move_vel_q_result_.result.error_code = 1;
    move_vel_q_result_.result.error_msg = "moveVelQ failed to execute";
    move_vel_q_result_.result.succeeded = static_cast<unsigned char>(false);
    ROS_WARN("move_vel_q: Failed");
    as_move_vel_q_.setAborted(move_vel_q_result_, "moveVelQ failed to execute");
  }
}

void URActionInterface::moveVelTCB(const caros_universalrobot::MoveVelTGoalConstPtr &goal)
{
  bool success = true;

  double acceleration = goal->acceleration;
  double time = goal->time;
  rw::math::VelocityScrew6D<> tcp_velocity = caros::toRw(goal->tcp_velocity);

  // publish info to the console for the user
  ROS_INFO_STREAM("move_vel_t: Executing, calling moveVelT with goal velocities: " << tcp_velocity);

  // start executing the action
  success = urMoveVelT(tcp_velocity, acceleration, time);

  if (success)
  {
    move_vel_t_result_.result.error_code = 0;
    move_vel_t_result_.result.error_msg = "";
    move_vel_t_result_.result.succeeded = static_cast<unsigned char>(true);
    ROS_INFO("move_vel_t: Succeeded!");
    // set the action state to succeeded
    as_move_vel_t_.setSucceeded(move_vel_t_result_);
  }
  else
  {
    move_vel_t_result_.result.error_code = 1;
    move_vel_t_result_.result.error_msg = "moveVelT failed to execute";
    move_vel_t_result_.result.succeeded = static_cast<unsigned char>(false);
    ROS_WARN("move_vel_t: Failed");
    as_move_vel_t_.setAborted(move_vel_t_result_, "moveVelT failed to execute");
  }
}

void URActionInterface::moveVelStopCB(const caros_universalrobot::MoveVelStopGoalConstPtr &goal)
{
  bool success = true;

  // publish info to the console for the user
  ROS_INFO("move_vel_stop: Executing, calling urMoveVelStop");

  // start executing the action
  success = urMoveVelStop();

  if (success)
  {
    move_vel_stop_result_.result.error_code = 0;
    move_vel_stop_result_.result.error_msg = "";
    move_vel_stop_result_.result.succeeded = static_cast<unsigned char>(true);
    ROS_INFO("move_vel_stop: Succeeded!");
    // set the action state to succeeded
    as_move_vel_stop_.setSucceeded(move_vel_stop_result_);
  }
  else
  {
    move_vel_stop_result_.result.error_code = 1;
    move_vel_stop_result_.result.error_msg = "urMoveVelStop failed to execute";
    move_vel_stop_result_.result.succeeded = static_cast<unsigned char>(false);
    ROS_WARN("move_vel_stop: Failed");
    as_move_vel_stop_.setAborted(move_vel_stop_result_, "urMoveVelStop failed to execute");
  }
}

void URActionInterface::setIOCB(const caros_universalrobot::SetIOGoalConstPtr &goal)
{
  bool success = true;

  int pin = goal->pin;
  bool value = goal->value;

  // publish info to the console for the user
  ROS_INFO_STREAM("set_io: Executing, calling urSetIO with pin " << pin << " = " << (value ? "true" : "false"));

  // start executing the action
  success = urSetIO(pin, value);

  if (success)
  {
    set_io_result_.result.error_code = 0;
    set_io_result_.result.error_msg = "";
    set_io_result_.result.succeeded = static_cast<unsigned char>(true);
    ROS_INFO("set_io: Succeeded!");
    // set the action state to succeeded
    as_set_io_.setSucceeded(set_io_result_);
  }
  else
  {
    set_io_result_.result.error_code = 1;
    set_io_result_.result.error_msg = "urSetIO failed to execute";
    set_io_result_.result.succeeded = static_cast<unsigned char>(false);
    ROS_WARN("set_io: Failed");
    as_set_io_.setAborted(set_io_result_, "urSetIO failed to execute");
  }
}

void URActionInterface::setPayloadCB(const caros_universalrobot::SetPayloadGoalConstPtr &goal)
{
  bool success = true;

  double tcp_weight = goal->tcp_weight;
  rw::math::Vector3D<> center_of_mass;
  center_of_mass[0] = 0;
  center_of_mass[1] = 0;
  center_of_mass[2] = 0;

  // publish info to the console for the user
  ROS_INFO_STREAM("set_payload: Executing, calling urSetPayload with payload: " << tcp_weight);

  // start executing the action
  success = urSetPayload(tcp_weight, center_of_mass);

  if (success)
  {
    set_payload_result_.result.error_code = 0;
    set_payload_result_.result.error_msg = "";
    set_payload_result_.result.succeeded = static_cast<unsigned char>(true);
    ROS_INFO("set_payload: Succeeded!");
    // set the action state to succeeded
    as_set_payload_.setSucceeded(set_payload_result_);
  }
  else
  {
    set_payload_result_.result.error_code = 1;
    set_payload_result_.result.error_msg = "urSetPayload failed to execute";
    set_payload_result_.result.succeeded = static_cast<unsigned char>(false);
    ROS_WARN("set_payload: Failed");
    as_set_payload_.setAborted(set_payload_result_, "urSetPayload failed to execute");
  }
}

void URActionInterface::zeroFTsensorCB(const caros_universalrobot::ZeroFTsensorGoalConstPtr &goal)
{
  bool success = true;

  // publish info to the console for the user
  ROS_INFO_STREAM("zero_ftsensor: Executing");

  // start executing the action
  success = urZeroFtSensor();

  if (success)
  {
    zero_ftsensor_result_.result.error_code = 0;
    zero_ftsensor_result_.result.error_msg = "";
    zero_ftsensor_result_.result.succeeded = static_cast<unsigned char>(true);
    ROS_INFO("zero_ftsensor: Succeeded!");
    // set the action state to succeeded
    as_zero_ftsensor_.setSucceeded(zero_ftsensor_result_);
  }
  else
  {
    zero_ftsensor_result_.result.error_code = 1;
    zero_ftsensor_result_.result.error_msg = "urZeroFtSensor failed to execute";
    zero_ftsensor_result_.result.succeeded = static_cast<unsigned char>(false);
    ROS_WARN("zero_ftsensor: Failed");
    as_zero_ftsensor_.setAborted(zero_ftsensor_result_, "urZeroFtSensor failed to execute");
  }
}

void URActionInterface::sendCustomScriptFunctionCB(
    const caros_universalrobot::SendCustomScriptFunctionGoalConstPtr &goal)
{
  bool success = true;

  std::string function_name = goal->function_name;
  std::string script = goal->script;

  // publish info to the console for the user
  ROS_INFO_STREAM("send_custom_script_function: Executing script function with name: " << function_name);

  // start executing the action
  success = urSendCustomScriptFunction(function_name, script);

  if (success)
  {
    send_custom_script_function_result_.result.error_code = 0;
    send_custom_script_function_result_.result.error_msg = "";
    send_custom_script_function_result_.result.succeeded = static_cast<unsigned char>(true);
    ROS_INFO("send_custom_script_function: Succeeded!");
    // set the action state to succeeded
    as_send_custom_script_function_.setSucceeded(send_custom_script_function_result_);
  }
  else
  {
    send_custom_script_function_result_.result.error_code = 1;
    send_custom_script_function_result_.result.error_msg = "urSendCustomScriptFunction failed to execute";
    send_custom_script_function_result_.result.succeeded = static_cast<unsigned char>(false);
    ROS_WARN("send_custom_script_function: Failed");
    as_send_custom_script_function_.setAborted(send_custom_script_function_result_,
                                               "urSendCustomScriptFunction failed to execute");
  }
}

void URActionInterface::sendCustomScriptFileCB(const caros_universalrobot::SendCustomScriptFileGoalConstPtr &goal)
{
  bool success = true;

  std::string file_path = goal->file_path;

  // publish info to the console for the user
  ROS_INFO_STREAM("send_custom_script_file: Executing script file: " << file_path);

  // start executing the action
  success = urSendCustomScriptFile(file_path);

  if (success)
  {
    send_custom_script_file_result_.result.error_code = 0;
    send_custom_script_file_result_.result.error_msg = "";
    send_custom_script_file_result_.result.succeeded = static_cast<unsigned char>(true);
    ROS_INFO("send_custom_script_file_result: Succeeded!");
    // set the action state to succeeded
    as_send_custom_script_file_.setSucceeded(send_custom_script_file_result_);
  }
  else
  {
    send_custom_script_file_result_.result.error_code = 1;
    send_custom_script_file_result_.result.error_msg = "urSendCustomScriptFile failed to execute";
    send_custom_script_file_result_.result.succeeded = static_cast<unsigned char>(false);
    ROS_WARN("send_custom_script_file: Failed");
    as_send_custom_script_file_.setAborted(send_custom_script_file_result_, "urSendCustomScriptFile failed to execute");
  }
}
